namespace Microsoft.Robotics.Navigation.Motion
{
    using System;
    using System.Collections.Generic;
    using System.Drawing;
    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Navigation.Motion;
    using Microsoft.Robotics.Navigation.Perception;
    using Microsoft.Robotics.Tracing;
    using MicrosoftResearch.Infer.Distributions;

    /// <summary>
    /// Implements low level suggestion drive logic and state machine intended to run on the Ev4 robot locally. 
    /// </summary>
    public class Ev4LocalSuggestionDrive
    {
        /// <summary>
        /// Decimation ratio when scanning the depth frame. Not externalized, as this value is not interesting to play with.
        /// </summary>
        public const int DepthFrameSkipRateAt640x480Resolution = 8;

        /// <summary>
        /// We don't consider sides for close readings. Value of 4 means: ignore 1/4 of image from both sides of the frame
        /// </summary>
        public const int SidePortionOfTheFrameToIgnoreForCloseReadings = 4;        

        /// <summary>
        /// Kinect camera specific dead columns from the left
        /// </summary>
        private const int DepthFrameDeadColumnsAt640x480Resolution = 8;

        /// <summary>
        /// To avoid reacting to IR shadows cast by tall objects when viewed from the right, we must discard thin sections of no readings
        /// </summary>
        private const int MinConsecutiveNoReadingColumnsAsFractionOfWidth = 8;

        /// <summary>
        /// OA trace context
        /// </summary>
        private static TraceContext oactxt = TraceContexts.FromString(TraceContexts.Nav, "ObstAv");

        /// <summary>
        /// This is a 'bump' distance for sonars
        /// </summary>
        private double sonarThresholdDistanceInMeters;

        /// <summary>
        /// This is a 'bump' distance for kinect
        /// </summary>
        private int depthThresholdInMM;

        /// <summary>
        /// After avoiding an obstacle we drive forward some random amount of time. This is the configured max of that random value.
        /// </summary>
        private int maxMillisecondsDrivingForwardAfterTurning;

        /// <summary>
        /// Overshoot when turning away from obstacles in milliseconds
        /// </summary>
        private int desiredOverShootMillisecondsWhileTurning;      
        
        /// <summary>
        /// The speed at which 
        /// </summary>
        private double driveForwardSpeedAfterAvoiding;

        /// <summary>
        /// Probability with which we will respect suggestion from the upstream component on the direction of escaping an obstacle.
        /// </summary>
        private double probabilityOfRespectingSuggestedDirection;

        /// <summary>
        /// Single instance of random variable ensures we are seeded and will get unique values (otherwise, internally, random number generator is re-seeded at fairly low frame rate)
        /// </summary>
        private Random rand;
        
        /// <summary>
        /// Initializes a new instance of the <see cref="Ev4LocalSuggestionDrive" /> class
        /// </summary>
        /// <param name="sonarThresholdDistanceInMM">Sonar threshold</param>
        /// <param name="depthThresholdInMM">Depth threshold</param>
        /// <param name="desiredOverShootMillisecondsWhileTurning">When turning away from an obstacle to avoid it - we overshoot to ensure we have clearance before driving straight</param>
        /// <param name="maxMillisecondsDrivingForwardAfterTurning">After avoiding an obstacle we drive forward some random amount of time. This is the max of that random.</param>
        /// <param name="driveForwardSpeedAfterAvoiding">Speed at which we will be driving straight after avoiding an obstacle.</param>
        /// <param name="probabilityOfRespectingSuggestedDirection">Probability with which we will respect suggestion from the upstream component on the direction of escaping an obstacle.</param>
        public Ev4LocalSuggestionDrive(
            int sonarThresholdDistanceInMM, 
            int depthThresholdInMM, 
            int desiredOverShootMillisecondsWhileTurning, 
            int maxMillisecondsDrivingForwardAfterTurning,
            double driveForwardSpeedAfterAvoiding,
            double probabilityOfRespectingSuggestedDirection) 
        {
            // we keep distance internally in Meters, since that's what sensor reports, and it makes code cleaner
            // but we expose interface in millimeters for consistency with Kinect threshold
            this.sonarThresholdDistanceInMeters = (double)sonarThresholdDistanceInMM / 1000;
            this.depthThresholdInMM = depthThresholdInMM;
            this.desiredOverShootMillisecondsWhileTurning = desiredOverShootMillisecondsWhileTurning;
            this.maxMillisecondsDrivingForwardAfterTurning = maxMillisecondsDrivingForwardAfterTurning;
            this.driveForwardSpeedAfterAvoiding = driveForwardSpeedAfterAvoiding;
            this.probabilityOfRespectingSuggestedDirection = probabilityOfRespectingSuggestedDirection;

            this.rand = new Random();
        }        

        /// <summary>
        /// Gets Obstacle Avoidance trace context
        /// </summary>
        public static TraceContext OATraceCtxt
        {
            get { return oactxt; }
        }        

        /// <summary>
        /// Produces safe speed after analyzing depth frame, taking into account the state and the original input (desired) velocity
        /// </summary>
        /// <param name="state">OA state</param>
        /// <param name="suggestedVelocity">Suggested input velocity</param>                
        /// <returns>Safe velocity, which will be same as suggested velocity if no local obstacles are detected</returns>
        public MobileBaseVelocityReference SafeSpeedFromSuggestedSpeed(
            LocalObstacleAvoidanceState state,
            MobileBaseVelocityReference suggestedVelocity)
        {
            if (null == state || null == state.LastDepthImage || null == suggestedVelocity)
            {
                return suggestedVelocity;
            }

            double kinectVote = this.GetNearestObstacleDirectionFromDepthFrame(state.LastDepthImage, state.LastDepthImageSize);

            double sonarVote = this.GetNearestObstacleDirectionFromSonar(state.LastSonarState);

            double finalDirection = this.FuseSensorVotes(kinectVote, sonarVote);

            return this.ProduceObstacleEscapeVelocity(
                state, 
                suggestedVelocity,
                finalDirection,
                this.rand);
        }

        /// <summary>
        /// Fuse sensor votes on avoiding obstacles. Sonar is given higher priority than Kinect due to higher
        /// reliability (directness) of sonar sensor measurement.
        /// </summary>
        /// <param name="kinectDir">Kinect vote</param>
        /// <param name="sonarDir">Sonar vote</param>
        /// <returns>Nearest obstacle direction or 0 if no obstacles are seen</returns>
        public double FuseSensorVotes(double kinectDir, double sonarDir) 
        {
            if (0 != sonarDir)
            {
                TraceOut.Info(OATraceCtxt, "Sonar {0} kicked in", sonarDir); 

                return sonarDir;
            }

            return kinectDir;
        }

        /// <summary>
        /// Decide direction of the obstacle, given sonar state. Stateless.
        /// </summary>
        /// <param name="sonarState">Sonar state</param>        
        /// <returns>Nearest obstacle direction or 0 if no obstacles are seen</returns>
        public double GetNearestObstacleDirectionFromSonar(List<SonarState> sonarState)
        {
            if (null != sonarState && 2 == sonarState.Count)
            {
                double leftDist = sonarState[0].DistanceInMeters;
                double rightDist = sonarState[1].DistanceInMeters;

                // We should treat 0 as "infinity" as it may indicate both too close obstacle or a long corridor
                if (0 == leftDist) 
                {
                    leftDist = double.MaxValue;
                }

                if (0 == rightDist)
                {
                    rightDist = double.MaxValue;
                }

                // Both sonars may have fired - take the one with closest distance
                if (leftDist < this.sonarThresholdDistanceInMeters && leftDist <= rightDist)
                {
                    TraceOut.Info(OATraceCtxt, "OA: Left sonar fired below threshold. Distance {0}.", leftDist);
                    return -1;
                }

                if (rightDist < this.sonarThresholdDistanceInMeters && rightDist <= leftDist)
                {
                    TraceOut.Info(OATraceCtxt, "OA: Right sonar fired below threshold. Distance {0}.", rightDist);
                    return 1;
                }
            }

            return 0;
        }

        /// <summary>
        /// Decide direction of the obstacle, given a depth frame. Stateless.
        /// </summary>
        /// <param name="depthImage">Depth image</param>
        /// <param name="imageSize">Depth image size</param>
        /// <returns>Nearest obstacle direction or 0 if no obstacles are seen</returns>
        public double GetNearestObstacleDirectionFromDepthFrame(short[] depthImage, Size imageSize)
        {
            int frameScanningStep = DepthFrameSkipRateAt640x480Resolution / (640 / imageSize.Width);
            int kinectDeadPixelsFromLeft = DepthFrameDeadColumnsAt640x480Resolution / (640 / imageSize.Width);

            int[] obstacleCountInAColumn = new int[imageSize.Width / frameScanningStep];
            int[] noReadingsCountInAColumn = new int[imageSize.Width / frameScanningStep];

            for (int i = 0; i < imageSize.Height; i += frameScanningStep)
            {
                int stride = i * imageSize.Width;

                for (int j = kinectDeadPixelsFromLeft; j < imageSize.Width; j += frameScanningStep)
                {
                    short depth = depthImage[stride + j];

                    if (depth == 0 && i >= imageSize.Height / 2)
                    {
                        ++noReadingsCountInAColumn[j / frameScanningStep];
                    }
                    
                    if (depth > 0 && depth < this.depthThresholdInMM)
                    {
                        if (j > imageSize.Width / SidePortionOfTheFrameToIgnoreForCloseReadings ||
                            j < (imageSize.Width - imageSize.Width / SidePortionOfTheFrameToIgnoreForCloseReadings))
                        {
                            ++obstacleCountInAColumn[j / frameScanningStep];
                        }
                    }
                }
            }

            double obstacleDirection = 0;

            int consecutiveNoReadingColumns = 0;

            for (int j = kinectDeadPixelsFromLeft; j < imageSize.Width; j += frameScanningStep)
            {
                int indexInObstacleMaps = j / frameScanningStep;

                if (noReadingsCountInAColumn[indexInObstacleMaps] * frameScanningStep >= (imageSize.Height / 2 - 1))
                {
                    ++consecutiveNoReadingColumns;
                }
                else
                {
                    consecutiveNoReadingColumns = 0;
                }

                bool suspectObstacleDueToNoReadings = false;
                bool seeingObstacle = false;

                if (consecutiveNoReadingColumns * frameScanningStep > imageSize.Width / MinConsecutiveNoReadingColumnsAsFractionOfWidth) 
                {
                    suspectObstacleDueToNoReadings = true;
                }

                if (obstacleCountInAColumn[indexInObstacleMaps] > 0)
                {
                    seeingObstacle = true;
                }

                if (suspectObstacleDueToNoReadings || seeingObstacle)
                {
                    if (j < imageSize.Width / 2)
                    {
                        obstacleDirection = 1;
                    }
                    else
                    {
                        obstacleDirection = -1;
                    }

                    TraceOut.Info(
                        OATraceCtxt,
                        "Kinect OA triggered. Obstacle Direction:{0} Column:{1}. Reason: " + 
                            (string)(suspectObstacleDueToNoReadings ? "Suspect Obstacle due to no readings " : string.Empty) +
                            (string)(seeingObstacle ? "Seing obstacle" : string.Empty), 
                        obstacleDirection,
                        j);                         
                    
                    break;
                }
            }

            return obstacleDirection;
        }

        /// <summary>
        /// Combine state, suggested and safe speeds and produce final speed that would avoid obstacles while not getting stuck in dumb ways
        /// </summary>
        /// <param name="state">OA state</param>
        /// <param name="suggestedVelocity">Input suggestion velocity</param>
        /// <param name="obstacleDirection">Stateless direction of the obstacle</param>
        /// <param name="rand">Random number generator. We pass this from outside to be able to test behavior of this function by injecting a random value</param>
        /// <returns>Final OA speed that takes state into account</returns>
        public MobileBaseVelocityReference ProduceObstacleEscapeVelocity(
            LocalObstacleAvoidanceState state,
            MobileBaseVelocityReference suggestedVelocity,
            double obstacleDirection,
            Random rand)
        {
            double directionToUse = obstacleDirection;

            // To prevent jutter, we must keep yet another state that keeps track of previous frame, and makes sure we go into OA mode 
            // only if more than one consecutive frame voted "yes" on triggering OA
            if (directionToUse == 0)
            {
                state.ConsecutiveFramesWithObstacles = 0;
            }
            else
            {
                ++state.ConsecutiveFramesWithObstacles;

                state.LastTimeObstacleSeen = state.CurrentTimeStampInMS;
            }

            if (state.ConsecutiveFramesWithObstacles > 1 && suggestedVelocity.XInMetersPerSecond > 0) 
            {
                int requestedDirection = Math.Sign(suggestedVelocity.ThetaInRadiansPerSecond);
                
                // There is really no telling if respecting suggested direction from the upstream (and presumably smarter) controller (i.e. planner)
                // is always a good thing, or avoiding an obstacle in the naive way (away from it) is a better way to go. Therefore, we randomize
                // the desicion making. Experimetns show that ignoring suggested direction in majority of cases, and going away from obstacles (i.e. 70%) 
                // yields generally good results
                bool respectSuggestedDirectionWhenAvoidingObstacles = rand.NextDouble() < this.probabilityOfRespectingSuggestedDirection;

                if (respectSuggestedDirectionWhenAvoidingObstacles && 0 != requestedDirection)
                {
                    // Respect direction requested externally (i.e. by path planner), but maintain fixed escape speed. 
                    // If no direction is fed externally, or flag is set to ignore suggested direction - we escape in the 
                    // oppostite direction from nearest obstacle.
                    directionToUse = requestedDirection;
                }

                // Very basic method of getting untangled: rotate in the oppostite direction after having logged a full circle
                // Hopefully this will result in other forces pushing the robot away from the stuck state
                if (null != state.LastSafeVelocity) 
                {
                    double secondsPerFullTurn = (Math.PI * 2) / Math.Abs(state.LastSafeVelocity.ThetaInRadiansPerSecond);

                    if (state.MillisecondsSinceLastOASessionStarted > secondsPerFullTurn * 1000)
                    {
                        TraceOut.Warning(OATraceCtxt, "Suspect tangled castor, changing rotation direction to untangle.");

                        directionToUse = -Math.Sign(state.LastSafeVelocity.ThetaInRadiansPerSecond);
                    }
                    else
                    {
                        // Keep avoiding obstacles until expiry, clearing or direction change
                        return state.LastSafeVelocity;
                    }
                }

                // Start OA session, stop, pick rotation direction and spin
                return state.CreateNewSafeVelocity(directionToUse);                
            }
            else
            {
                if (null != state.LastSafeVelocity)
                {
                    // This is to prevent 'jitter' and ensure some amount of overshoot when avoiding obstacles. 
                    // Needed for smooth motion and more robust clearing of obstacles
                    if (state.MillisecondsSinceLastObstacleWasSeen < this.desiredOverShootMillisecondsWhileTurning)
                    {
                        // While still turning away from obstacle - decide how long we will be driving forward once the clearning appears
                        // Makes sense to calculate that value here. 
                        // We use max of 2 rands method here to decrease the chance of very small numbers but still keep it random 
                        // (simpler than gaussian with similar effect)
                        int rand1 = rand.Next(this.maxMillisecondsDrivingForwardAfterTurning);
                        int rand2 = rand.Next(this.maxMillisecondsDrivingForwardAfterTurning);

                        state.RandomTimeInMSToDriveStraightAfterClearing = Math.Max(rand1, rand2);

                        return state.LastSafeVelocity;
                    }
                    else if (state.MillisecondsSinceLastObstacleWasSeen < this.desiredOverShootMillisecondsWhileTurning + state.RandomTimeInMSToDriveStraightAfterClearing)
                    {
                        return new MobileBaseVelocityReference(this.driveForwardSpeedAfterAvoiding, 0, 0);
                    }
                }
            }

            // Clear, pass original suggested velocity through
            state.LastSafeVelocity = null;

            return suggestedVelocity;
        }

        /// <summary>
        /// Internal Obstacle avoidance state
        /// </summary>
        public class LocalObstacleAvoidanceState
        {
            /// <summary>
            /// Min escape rotation velocity. 
            /// </summary>
            private double escapeRotationRadsPerSecMin;

            /// <summary>
            /// Max escape rotation velocity. 
            /// </summary>
            private double escapeRotationRadsPerSecMax;

            /// <summary>
            /// Random number generator
            /// </summary>
            private Gaussian gaussian;

            /// <summary>
            /// Initializes a new instance of the <see cref="LocalObstacleAvoidanceState" /> class
            /// </summary>            
            /// <param name="escapeRotationRadsPerSecMin">Escape rotation max velocity</param>
            /// <param name="escapeRotationRadsPerSecMax">Escape rotation min velocity</param>
            /// <param name="escapeRotationRadsPerSecVariance">Escape rotation variance</param>
            public LocalObstacleAvoidanceState(
                double escapeRotationRadsPerSecMin,
                double escapeRotationRadsPerSecMax,
                double escapeRotationRadsPerSecVariance)
            {
                this.LastTimeOASessionCreated = 0;
                this.ConsecutiveFramesWithObstacles = 0;
                this.escapeRotationRadsPerSecMin = escapeRotationRadsPerSecMin;
                this.escapeRotationRadsPerSecMax = escapeRotationRadsPerSecMax;

                this.gaussian = Gaussian.FromMeanAndVariance(0, escapeRotationRadsPerSecVariance);
            }

            /// <summary>
            /// Gets or sets the safe velocity calculated based on depth camera readings when robot gets in dangerous proximity from an obstacle. 
            /// Does not respect any smoothing rules - if this value is set, it means it has to be considered above smooth trajectory speed.
            /// Calculated locally on a robot on each Kinect frame as those arrive.
            /// </summary>
            public MobileBaseVelocityReference LastSafeVelocity { get; set; }

            /// <summary>
            /// Gets or sets the latest depth image.
            /// </summary>
            public short[] LastDepthImage { get; set; }

            /// <summary>
            /// Gets or sets the latest depth image.
            /// </summary>
            public Size LastDepthImageSize { get; set; }

            /// <summary>
            /// Gets or sets the last timestamp when OA session was initiated. Session is between start and end of avoiding one obstacle.
            /// </summary>
            public long LastTimeOASessionCreated { get; set; }

            /// <summary>
            /// Gets or sets the last timestamp when obstacle was seen.
            /// </summary>
            public long LastTimeObstacleSeen { get; set; }

            /// <summary>
            /// Gets or sets the random number for the straight driving session
            /// </summary>
            public long RandomTimeInMSToDriveStraightAfterClearing { get; set; }

            /// <summary>
            /// Gets or sets the number of frames in a row that OA has kicked in
            /// We need to track this to have some time-averaging before applying OA for smoothness
            /// </summary>
            public long ConsecutiveFramesWithObstacles { get; set; }

            /// <summary>
            /// Gets or sets current escape rotation speed
            /// </summary>
            public double CurrentEscapeVelocity { get; set; }

            /// <summary>
            /// Gets or sets last sonar array state
            /// </summary>
            public List<SonarState> LastSonarState { get; set; }

            /// <summary>
            /// Gets or sets current time
            /// </summary>
            public long CurrentTimeStampInMS { get; set; }

            /// <summary>
            /// Gets amount of time passed since last OA session was triggered
            /// </summary>
            public long MillisecondsSinceLastOASessionStarted 
            {
                get 
                {
                    return this.CurrentTimeStampInMS - this.LastTimeOASessionCreated;
                }
            }

            /// <summary>
            /// Gets amount of time passed since last time an obstacle was seen
            /// </summary>
            public long MillisecondsSinceLastObstacleWasSeen
            {
                get
                {
                    return this.CurrentTimeStampInMS - this.LastTimeObstacleSeen;
                }
            }

            /// <summary>
            /// Creates new safe randomized velocity instance and keeps time of that creation
            /// </summary>
            /// <param name="direction">Desired rotation direction to use</param>            
            /// <returns>New instance of safe velocity</returns>
            public MobileBaseVelocityReference CreateNewSafeVelocity(double direction)
            {
                if (0 == direction)
                {
                    // this should never happen, we only need to sample the gaussian, etc if we are about to avoid an obstacle
                    // so instead of trying to add smarts to cover up potential bugs upstream - we throw to catch those bugs
                    throw new InvalidOperationException("No escape direction specified. This is a bug.");
                } 

                // We use randomized velocity in desired direction to disturb the system and hopefully avoid infinite oscilations
                // Get a rand around 0 with specified variance. We use gaussian, and not rand to ensure a tighter packing around min
                // speed, which results in sane turning behaviour
                double rand = this.gaussian.Sample();                

                // Need to respect requested direction. We also square the gaussian to achieve even tighter packing 
                // around the min (and also ensure positive sign). It results in relatively few (but still some) high speed dashes away from
                // obstacle or from stuck caster case
                double rotationVelocity = Math.Sign(direction) * (this.escapeRotationRadsPerSecMin + rand * rand);

                if (Math.Abs(rotationVelocity) > this.escapeRotationRadsPerSecMax)
                {
                    rotationVelocity = Math.Sign(rotationVelocity) * this.escapeRotationRadsPerSecMax;
                }

                TraceOut.Info(Ev4LocalSuggestionDrive.OATraceCtxt, "OA kicked in. New Rotation: {0}", rotationVelocity); 

                this.LastTimeOASessionCreated = this.CurrentTimeStampInMS;
                this.LastSafeVelocity = new MobileBaseVelocityReference(0, 0, rotationVelocity);

                return this.LastSafeVelocity;
            }
        }
    }
}
